// Have include guard.
#ifndef TWOJOYSTICKDRIVECONTROLLER_H
#define TWOJOYSTICKDRIVECONTROLLER_H

// Include files.
#include <vector>
#include "WPILib.h"
#include "Modifier.hpp"
#include "RobotController.hpp"

/**
 * The TwoJoystickDriveController drives using input from two joysticks.
 * 
 * @tparam Drive Needs to have a function called Stop that takes no arguments.
 * @tparam Handler
 */
template
<
	typename Drive,
	typename Value,
	Value(*Fetcher)(GenericHID &leftJoystick, GenericHID &rightJoystick),
	void(*Handler)(Drive &drive, Value &value)
>
class TwoJoystickDriveController :
	public RobotController
{
	public:
		typedef std::vector<Modifier<Value>*> ModiferList;
	protected:
		Drive &m_drive;
		GenericHID &m_joystickLeft;
		GenericHID &m_joystickRight;
		ModiferList m_modifiers;
	public:
		TwoJoystickDriveController(Drive &drive, GenericHID &leftJoystick, GenericHID &rightJoystick) :
			m_drive(drive),
			m_joystickLeft(leftJoystick),
			m_joystickRight(rightJoystick),
			m_modifiers()
		{
		}
		
		ModiferList &GetModifiers()
		{
			return m_modifiers;
		}
		
		const ModiferList &GetModifiers() const
		{
			return m_modifiers;
		}
	protected:
		virtual void Periodic()
		{			
			Value value((Fetcher(m_joystickLeft, m_joystickRight)));
			
			typename ModiferList::iterator it;
			
			for (it = m_modifiers.begin(); it != m_modifiers.end(); ++it)
				(*it)->Modify(value);
			
			Handler(m_drive, value);
		}
};

#endif // #ifndef ASYNCRESULT_H 
